/*
 * Servo.cpp
 *
 *  Created on: May 4, 2010
 *      Author: tflanzer
 */

#include <iostream>
#include "Servo.h"
using namespace std;

Servo::Servo(double a1, double a2, double a3, double a4, double max) {
	p1 = a1;
	p2 = a2;
	p3 = a3;
	p4 = a4;
	maxDeflection = max;
}

// Destructor
Servo::~Servo() {
}

void Servo::percentThrottle(double throttle) {
	if (throttle > 1) {
		throttle = 1;
		cout << "Commanded throttle exceeds limit" << endl;
	}
	else if (throttle < 0) {
		throttle = 0;
		cout << "Commanded throttle below zero" << endl;
	}
}

double Servo::actuateRad(double angle) {

	if (angle > maxDeflection) {
		angle = maxDeflection;
		cout << "Commanded servo deflection exceeds limit" << endl;
	}
	else if (angle < -maxDeflection) {
		angle = -maxDeflection;
		cout << "Commanded servo deflection exceeds limit" << endl;
	}
//	double pwm_low_lim = 900;
//	double pwm_range   = 1200;

	double pwm = p1*pow(angle,3)+p2*pow(angle,2)+p3*pow(angle,1)+p4;
//	double pwm = ((p1*pow(angle,3)+p2*pow(angle,2)+p3*pow(angle,1)+p4) - pwm_low_lim)/pwm_range*255;
	return pwm;
}
